/*#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     HTPB,                sensorI2CCustom9V)
#pragma config(Sensor, S3,     SMUX_1,              sensorI2CCustom)
#pragma config(Sensor, S4,     SMUX_2,              sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     e,             tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     a,             tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     b,             tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     c,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C3_1,     f,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     d,             tmotorNormal, PIDControl, encoder)
#pragma config(Servo,  srvo_S1_C4_1,    frontarm,             tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    backarm,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    dispencer,            tServoStandard)
#pragma config(Servo,  srvo_S1_C4_4,    Irpullup,             tServoStandard)
#pragma config(Servo,  srvo_S1_C4_5,    Irfront,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_6,    Irback,               tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*/

 /**********************************************************************\
*																																	  	 *
* PROGRAM: Big Board  	      														        	  *
* VERSION: 1.2																										    *
* AUTHOR: Team R.A.B.B.I                                              *
* DATE:		 April 2011  	  																					  *
*																																		  *
* LICENSE: GNU GPL V3 2011                                            *
\**********************************************************************/

#ifndef BIG_BOARD
#define BIG_BOARD
#endif

//Definitions for the bridge-lowering servos
#define SERVO_FRONTARM_UP 32
#define SERVO_FRONTARM_DOWN 182

//Definitions for the different hieghts of the arms
#define MIN_HEIGHT_ARM 300
#define DISPENCING_HEIGHT 559
#define RETRIEVING_HIGH 690
#define RETRIEVING_MIDDLE 475

// Definitions for EOPD Allign

#define DISTANCE_TO_WALL 19
#define DISTANCE_OF_DISPENSOR 81
#define SPEED_MOVEMENTS 8


//Definitions for the different sensor ports
#define COMPASSPOS msensor_S4_2
#define GYROPOS msensor_S4_1
#define ACCEL msensor_S4_3
#define IR_FRONT msensor_S3_1
#define IR_BACK msensor_S3_2
#define EOPD_R msensor_S3_4
#define EOPD_L msensor_S3_3
#define POTPOS 4

#define WHEEL_IR_FL 1
#define WHEEL_IR_FR 3
#define WHEEL_IR_BL 0
#define WHEEL_IR_BR 2


//Definitions for moving straight
#define A_FORWARD 0.80
#define B_FORWARD 0.85
#define C_FORWARD 1
#define D_FORWARD 0.68

#define A_BACKWARD 0.82
#define B_BACKWARD 0.90
#define C_BACKWARD 1
#define D_BACKWARD 0.70

// Main defines

#define RIGHTSD true
#define LEFTSD false

#define RED true
#define BLUE false

// Defines for Bridge program
#define SAMPLES_TOTAL 3
#define THROW_AWAY 2
#define EDGE 250

#define LOW_PASS_FACTOR 0.45

// after each round should check the the indevidaul values
#define EDGE_DIFF 70
#define EDGE_FL 285
#define HIGH_EDGE_FL EDGE_FL+EDGE_DIFF
#define EDGE_FR 290
#define HIGH_EDGE_FR EDGE_FR+EDGE_DIFF
#define EDGE_BL 315
#define HIGH_EDGE_BL EDGE_BL+EDGE_DIFF
#define EDGE_BR 290
#define HIGH_EDGE_BR EDGE_BR+EDGE_DIFF

#define ENCOD_CROSS 5200
#define E_CROSS_TRY_AGAIN 100
#define R_FIX 100
#define TRY_AGAIN_R_FIX 50

#define NOT_FALLING 0
#define FALLING_LEFT 1
#define FALLING_RIGHT 2

#define DRIVE_SPD CHARGE_SPD/1.5
#define CHARGE_SPD 37
#define SLOW_SPD CHARGE_SPD/3.5
#define SLOWER_SPD 1.5

#define IR_FL 1
#define IR_FR 2
#define IR_BL 4
#define IR_BR 8

#define FRICTION_BIAS 1.88 //1.88
#define BACK_BIAS 0.533
#define RIGHT_BIAS 0.857

// Defines for read mode EOPD

#define MAXIMUMDIST 275
